#ifndef __USER_PROG_
#define __USER_PROG_


#include "stdint.h"
#include "stdbool.h"
#include "Ano_ProgramCtrl_User.h"

//单位:10ms
#define MAX_COUNTDOWN 100 * 8
#define TARGETALTITUDECM 50
#define MAX_HOVER_ERR 10
#define MAX_FORMCHANGE_TIME 3
#define MAX_HOVER_TIME 300
#define MAX_TIMEOUT1 10000
#define MAX_TIMEOUT2 1500
#define MAX_ALT_ERR 10

#include "Ano_ProgramCtrl_User.h"

/*

*/

typedef enum
{
    CmdNone = 0,
    CmdTakeOff,
    CmdLand,
    CmdUp,
    CmdDown,
    CmdForward,
    CmdBack,
    CmdLeft,
    CmdRight,
    CmdCCW,
    CmdCW,
    CmdEmergencyShutDown,
    CmdSpeeedControl,

    NumofCmd,
} FMUCmd_t;



void Position_Init();
typedef enum
{
    ActionWaitting = 0,
    ActionCountdown,
    ActionTakeOff,
    ActionHoverStartPoint,
	
    Action_A0,
    Action_A1,
    Action_A2,
    Action_A3,
    Action_A4,
    Action_A5,
    Action_A6,
    Action_A7,
    Action_A8,
    Action_A9,

		Action_Land,
    NumofActionList,
} FSMList_t;


typedef struct
{
    int16_t x;
    int16_t y;
	  int16_t z;
    uint8_t code;
} Point_t;

//数据结构声明
// #pragma pack(1)
typedef struct
{
    Point_t Pos1;
    Point_t Pos2;
    Point_t Pos3;
    Point_t Pos4;
    Point_t Pos5;
    Point_t Pos6;
	uint8_t Name;
} Position;

typedef struct
{
    uint8_t A1;
    uint8_t A2;
    uint8_t A3;
    uint8_t A4;
    uint8_t A5;
    uint8_t A6;

    uint8_t B1;
    uint8_t B2;
    uint8_t B3;
    uint8_t B4;
    uint8_t B5;
    uint8_t B6;

    uint8_t C1;
    uint8_t C2;
    uint8_t C3;
    uint8_t C4;
    uint8_t C5;
    uint8_t C6;

    uint8_t D1;
    uint8_t D2;
    uint8_t D3;
    uint8_t D4;
    uint8_t D5;
    uint8_t D6;
} Good_Encode;

typedef struct
{
    int16_t Start;
    uint8_t Start2;
    Point_t CentPoint;
    int16_t End;
} OpenMVFrame_t;

#pragma pack()

typedef struct
{
    uint16_t SonarF;
    uint16_t SonarB;
    uint16_t SonarL;
    uint16_t SonarR;
} SonarManager_t;

typedef enum
{
    searching = 0,
    done,
    fail,
    frame_err,

    list_searching,
} enSearch_state_t;




typedef struct
{
    int CountDownNumMs;
    FSMList_t ActionList;
	FSMList_t Action_Last;
    FSMList_t Action_IRQ;
    float distance;
    bool ActionComplete;
} FollowManager_t;



//typedef struct
//{
//    //动作序列
//    ActList_t ActList;

//} ShelveManager_t;

extern FollowManager_t FollowManager;
extern SonarManager_t SonarManager;

void Follow_Init(void);
void UpdateStatus(float dT_s,s16 *CH_N);
void ActionIRQ_Request(FSMList_t Action_Req);

void Test_ProgramCtrl(float dT_s,s16 *CH_N);








#endif
